package Behaviors.Schemas.Perception;
import Robot.*;
import Robot.Sensors.IR;
import Robot.Sensors.Sensor;
import Util.Vector2d;
import Util.Pose2d;

public class PSDetectObstaclesIR extends PSDetectObstacles {
    private final float MIN_RANGE=0.1f;
    private final float MAX_RANGE=1.0f;
    
    IR ir;
    Position2d location;
    float [] poses;
    Vector2d[] data;
    Vector2d value=new Vector2d(0f,0f,Vector2d.MODE_XY);
    
    public PSDetectObstaclesIR(String name,Position2d location, IR ir) {
        super(name);
        
        this.ir=ir;
        this.location = location;

        poses = ir.getPoses();
        
        data = new Vector2d[poses.length];

        //assign to each vector of data, the angle of each IR in the robot.
        for(int i=0;i<poses.length;i++){
            //System.out.println(i);
            //System.out.println(poses[i]);
            data[i] = new Vector2d();
            data[i].setAngle(poses[i]);
        }
        
    }

    public Vector2d[] Value() {
        
        float [] dataTemp=ir.read(Sensor.READ_MODE_NO_WAIT);
        Pose2d locPose = location.read(Sensor.READ_MODE_NO_WAIT);

        //as with the IRs we have cannot sense distance ranges on NXBot, set vector mag. as 1.
        for(int i=0;i<data.length;i++){

            if(dataTemp[i]<=MIN_RANGE)
            {
                //if there is an obstacle, signal it. The location of the obstacle
                //is with respect to origin (0,0)

                //define theta with respect to global axes
                data[i].setAngle(poses[i]+ locPose.getPa());
                //add the robot radius to the distance
                data[i].setMag(MIN_RANGE + Robot.radius);
                //Translate vector from robot coordinates to global coordinates
                data[i] = data[i].add(location.getPositionVector());

//                System.out.println("PSDETECTOBSTACLESIR: Pos Robot");
//                location.getPositionVector().print(Vector2d.MODE_XY);
//                System.out.println("PSDETECTOBSTACLESIR: Obstaculo["+i+"]");
//                data[i].print(Vector2d.MODE_XY);

            }
                //if there is an obstacle, signal it in vector with zero
                //data[i].setMag(0);
            else
                data[i].setMag(0);
            
            
        }
        return data;
    }
}
